﻿using System;
using System.Collections.Generic;
using System.Windows.Media.Media3D;
using Coding4Fun.BoxingBots.Common;
using Microsoft.Kinect;

namespace Coding4Fun.BoxingBots.UserConsole
{
	public static class SkeletonProcessor
	{
		/* PUBLIC */
		private const double RangeNormalizer = .22;
		private const double NoiseClip = .05;
		public static Point3D CoreAverageDelta = new Point3D(0.0, 0.0, 0.0);
		public static Point3D FixedCenter = new Point3D(0.0, 0.0, 2.0);
		public static Point3D RingCenter = new Point3D(0.0, 0.0, 2.0);
		public static Skeleton RawSkeleton;
		public static Point3D PunchLeftDelta = new Point3D(0.0, 0.0, 0.0);
		public static Point3D PunchRightDelta = new Point3D(0.0, 0.0, 0.0);

		public static double PivotShoulders;
		public static double PivotHips;
		public static double PivotAverage;

		private static readonly List<JointType> CoreJoints = new List<JointType>(
			new[] {JointType.AnkleLeft, JointType.AnkleRight, JointType.ShoulderCenter, JointType.HipCenter}
			);

		private static Point3D _coreAverageTotal = new Point3D(0.0, 0.0, 0.0);

		private static bool _isPunchingRight;
		private static bool _isPunchingLeft;

		public static readonly double[] LeftPunchValues = new[] { 0d, 0d, 0d };
		public static readonly double[] RightPunchValues = new[] { 0d, 0d, 0d };

		private static readonly double[] LeftVelocities = new[] { 0d, 0d, 0d };
		private static readonly double[] LeftWingValues = new[] { 0d, 0d, 0d };

		private static readonly double[] RightVelocities = new[] {0d, 0d, 0d};
		private static readonly double[] RightWingValues = new[] {0d, 0d, 0d};

		public static double DistanceFromFixedCenter;


		public static MovementDescriptorPacket AnalyzeSkeleton(Skeleton skeleton)
		{
			var packet = new MovementDescriptorPacket();

			DistanceFromFixedCenter = DistanceFromRingCenter(skeleton);

			if (DistanceFromFixedCenter >= .75)
			{
				RawSkeleton = null;
				CoreAverageDelta = new Point3D(0.0, 0.0, 0.0);
				RingCenter = new Point3D(0.0, 0.0, 2.0);
				PunchLeftDelta = new Point3D(0.0, 0.0, 0.0);
				PunchRightDelta = new Point3D(0.0, 0.0, 0.0);
				_coreAverageTotal = new Point3D(0.0, 0.0, 0.0);
				PivotShoulders = 0.0;
				PivotHips = 0.0;
				PivotAverage = 0.0;
			}
			else
			{
				RawSkeleton = skeleton;

				_coreAverageTotal.X = 0.0;
				_coreAverageTotal.Z = 0.0;

				CoreAverageDelta.X = 0.0;
				CoreAverageDelta.Z = 0.0;

				foreach (JointType jt in CoreJoints)
				{
					_coreAverageTotal.X += skeleton.Joints[jt].Position.X;
					_coreAverageTotal.Z += skeleton.Joints[jt].Position.Z;

					CoreAverageDelta.X += skeleton.Joints[jt].Position.X - RingCenter.X;
					CoreAverageDelta.Z += skeleton.Joints[jt].Position.Z - RingCenter.Z;
				}

				CoreAverageDelta.X /= CoreJoints.Count * RangeNormalizer;
				CoreAverageDelta.Z /= CoreJoints.Count * RangeNormalizer;

				CoreAverageDelta.X = CheckBounds(CoreAverageDelta.X);
				CoreAverageDelta.Z = CheckBounds(CoreAverageDelta.Z);

				// CALCULATE PIVOT ANGLES
				PivotShoulders = CalculatePivotAngle(skeleton, JointType.ShoulderLeft, JointType.ShoulderRight);
				PivotHips = CalculatePivotAngle(skeleton, JointType.HipLeft, JointType.HipRight);

				PivotAverage = (PivotShoulders + PivotHips) / 2.0;

				packet.Rotate = PivotAverage;

				// CALCULATE ARM VALUES
				packet.PunchLeft = CalculatePunching(skeleton, JointType.WristLeft, JointType.ShoulderLeft,
				                                     PunchLeftDelta, LeftVelocities, LeftWingValues, LeftPunchValues,
				                                     ref _isPunchingLeft);

				packet.PunchRight = CalculatePunching(skeleton, JointType.WristRight, JointType.ShoulderRight,
				                                      PunchRightDelta, RightVelocities, RightWingValues, RightPunchValues,
				                                      ref _isPunchingRight);

				if (CoreAverageDelta.Z > NoiseClip || CoreAverageDelta.Z < -NoiseClip)
				{
					packet.Move = -CoreAverageDelta.Z;
				}

				if (CoreAverageDelta.X > NoiseClip || CoreAverageDelta.X < -NoiseClip)
				{
					packet.Strafe = CoreAverageDelta.X;
				}
			}

			return packet;
		}

		private static bool CalculatePunching(Skeleton skeleton, JointType wristJoint, JointType shoulderJoint,
		                                      Point3D punchDelta, double[] velocities, double[] wingValues,
		                                      double[] punchValues, ref bool oldPunchingState)
		{
			punchDelta.Z = skeleton.Joints[wristJoint].Position.Z - skeleton.Joints[shoulderJoint].Position.Z;
			punchDelta.Z /= (RangeNormalizer * 1.8);

			punchDelta.Y = skeleton.Joints[wristJoint].Position.Y - skeleton.Joints[JointType.Spine].Position.Y;
			punchDelta.Y = 1.1 * CheckBounds(punchDelta.Y / RangeNormalizer, 0);

			CalculatePunchWeighting(velocities, wingValues, punchValues, punchDelta);

			bool returnValue = false;
            if (punchDelta.Z < -.8) returnValue = true;

            /*
			if (punchValues[2] <= -1.0)
			{
				if (!oldPunchingState)
				{
					returnValue = true;
					oldPunchingState = true;
				}
			}
			else
			{
				oldPunchingState = false;
			}*/

			return returnValue;
		}

		private static double CalculatePivotAngle(Skeleton skeleton, JointType joint1, JointType joint2)
		{
			double value =
				(skeleton.Joints[joint1].Position.Z - skeleton.Joints[joint1].Position.Z) /
				(skeleton.Joints[joint1].Position.X - skeleton.Joints[joint2].Position.X);

			if (value > 0.0)
			{
				value = (value > .5) ? (Math.Min(value, 1.0) - .5) / .5 : 0.0;
			}
			else
			{
				value = (value < -.5) ? (Math.Max(value, -1.0) + .5) / .5 : 0.0;
			}

			return value;
		}

		private static double CheckBounds(double value, double lowerBound = -1d, double upperBound = 1d)
		{
			return Math.Min(upperBound, Math.Max(lowerBound, value));
		}

		public static double DistanceFromRingCenter(Skeleton skeletonData)
		{
			if (skeletonData == null)
			{
				return 10;
			}

			double dx = skeletonData.Joints[JointType.HipCenter].Position.X - FixedCenter.X;
			double dz = skeletonData.Joints[JointType.HipCenter].Position.Z - FixedCenter.Z;

			return Math.Sqrt(dx * dx + dz * dz);
		}

		private static void CalculatePunchWeighting(double[] velocities, double[] wingValues, double[] punchValues, Point3D punchDelta)
		{
			double extend = punchDelta.Z;
			double dampen = punchDelta.Y;
			
			double adjExtend = Math.Cos(1.57 * (Math.Min(Math.Max(extend, -1.0), 0.0)));

			velocities[0] = dampen * (adjExtend - wingValues[0]);
			wingValues[0] += velocities[0] * .2;
			punchValues[0] = (-1 + wingValues[0]) * dampen;

			double tempWingValueDelta = wingValues[1];

			CalculatePunchWingWeight(velocities, wingValues, punchValues, dampen, 1, velocities[0], .3, .35, .94);

			tempWingValueDelta = wingValues[1] - tempWingValueDelta;

			CalculatePunchWingWeight(velocities, wingValues, punchValues, dampen, 2, tempWingValueDelta, 1.4, .4, .94);
		}

		private static void CalculatePunchWingWeight(double[] velocities, double[] wingValues, double[] punchValues, double dampen,
			int indexOffset, double velocityMultiplierCheck, double velocityWeightMult, double wingWeightMult, double gravity)
		{
			if (velocityMultiplierCheck < 0.0)
			{
				velocities[indexOffset] += dampen * (velocityMultiplierCheck * velocityWeightMult);
			}

			velocities[indexOffset] *= gravity;

			wingValues[indexOffset] += velocities[indexOffset];
			wingValues[indexOffset] += (1.0 - wingValues[indexOffset]) * wingWeightMult;
			
			wingValues[indexOffset] = Math.Max(0.0, wingValues[indexOffset]);

			punchValues[indexOffset] = (-1 + wingValues[indexOffset]) * dampen;
		}
	}
}